
for i = 1:n
    qi = Q(i, :);
    x_real(3*i-2:3*i, 1) = robot_real.fkine(qi).t;
end

alpha = 0.01;
beta = 0.9;

iter_max = 20;
dtheta_k = zeros(1, 6);
finish = false;
for k = 1:iter_max
    
    for i = 1:n
        qi = Q(i, :);
        Jacobian(3*i-2:3*i, :) = calculate_Jacobian(dtheta_k, DH_para, qi);
    end

    error_x = calculate_error(dtheta_k, DH_para, x_real, Q);
    delta_theta = Jacobian(:, 1:5) \ error_x;
    
    tk = 1;
    while norm(calculate_error(dtheta_k + tk * [delta_theta', 0], DH_para, x_real, Q)) > norm(error_x)
        tk = beta * tk;
        if tk < 0.1
            finish = true;
            delta_theta = 0 * delta_theta;
            break
        end
    end
    
    dtheta_k = dtheta_k + tk * [delta_theta', 0];
    if finish
        break;
    end
    
end

robot.offset = robot.offset + dtheta_k;

disp("zero offset: ");
disp(dtheta_k);